#include "vec2.h"
#include "math.h"
#include "util/mmgr.h"		// Needs to come in early
#include "math_core.h"

using namespace math;

Vec2::Vec2(float _x,float _y): x(_x), y(_y)
{
}

Vec2::Vec2() : x(0), y(0)
{
}



Vec2 IntersectPlane(const Vec2& vNorm,float d,const Vec2& p1,const Vec2& p2)
{
	// returns the point where the line p1-p2 intersects the plane n&d
        Vec2 vDif	= p2 - p1;
        float	fDn		= vNorm^vDif;
        float	fT		= -(d+(vNorm^p1) )/fDn;
        return  p1 + (vDif*fT);
}




float	Vec2::Manhatten() const
{
	return (float)(fabs(x) + fabs(y));
}


Vec2& Vec2::Normalize() 
{
    float	fDistance = Magnitude();

    if (fDistance==0) 
	{
		// Error
		fDistance = 1.0f;
	}

	float	fInvD = 1.0f / fDistance;

	x *= fInvD;
    y *= fInvD;
    return *this;
}


int Vec2::Concurrent(const Vec2& _Vec) const
{
	return(x==_Vec.x && y==_Vec.y);
}


void Vec2::Reset()
{
	x = y = 0.0f;
}

void Vec2::Truncate(float MaxLength)
{
	float	Speed = MagSqr();
	if (Speed > MaxLength*MaxLength)
	{
		Normalize();
		x *= MaxLength;
		y *= MaxLength;
	}
}



void Vec2::Rotate(float Angle)
{

	Angle *= DEG_TO_RAD;

	float Sin = sinf(Angle);
	float Cos = cosf(Angle);

	float OldX = x;
	float OldY = y;

	x = (OldX*Cos)-(OldY*Sin);
	y = (OldX*Sin)+(OldY*Cos);

}


//working with angles
float Vec2::AngleAgainstVector(const Vec2& Vector)
{
	Vec2 tmpVec1(x,y);
	Vec2 tmpVec2(Vector.x, Vector.y);

	tmpVec1.Normalize();
	tmpVec2.Normalize();

	return(acos(tmpVec1^tmpVec2) * RAD_TO_DEG);

}


float Vec2::AngleAgainstXAxis()
{
	return AngleAgainstVector(Vec2(1.0f, 0.0f));
}


float Vec2::AngleAgainstYAxis()
{
	return AngleAgainstVector(Vec2(0.0f, 1.0f));
}


//reflecting against a normal
void Vec2::Reflect(const Vec2& NormalVector)
{

	float DoubleDotP = ((*this)^NormalVector) * 2.0f;

	x = x - DoubleDotP * NormalVector.x;
	y = y - DoubleDotP * NormalVector.y;

}





//operators
/*Vec2 operator+(const Vec2& vVec1,const Vec2& vVec2) 
{
	return Vec2( vVec1.x + vVec2.x,
					vVec1.y + vVec2.y);
}

Vec2 operator-(const Vec2& vVec1,const Vec2& vVec2) 
{
	return Vec2( vVec1.x - vVec2.x,
					vVec1.y - vVec2.y);
}*/

Vec2 operator-(const Vec2& vVec)            
{
	return Vec2( -vVec.x,
					-vVec.y);
}

/*Vec2 operator*(const Vec2&  vVec1, const Vec2& vVec2)
{
	return Vec2(	vVec1.x * vVec2.x, 
					vVec1.y * vVec2.y); 
}

Vec2 operator*(const Vec2& vVec1, float fScalar) 
{
	return Vec2(	vVec1.x * fScalar,
					vVec1.y * fScalar);
}


Vec2 operator*(float fScalar, const Vec2& vVec1)  
{
	return Vec2(	vVec1.x * fScalar,
					vVec1.y * fScalar);
}
*/

Vec2 operator/(const Vec2& vVec1,float fScalar)   
{
	float Div = (1.0f / fScalar);
	return Vec2(	vVec1.x * Div,
					vVec1.y * Div);
}



//Dotprod
float  operator^(const Vec2& vVec1,const Vec2& vVec2) 
{
	return	vVec1.x * vVec2.x 
		+	vVec1.y * vVec2.y;
}

// Op= functions

Vec2& Vec2::operator+= ( const Vec2& _Vec) 
{
	x += _Vec.x;
	y += _Vec.y;
     return ( *this ) ;
}

Vec2& Vec2::operator-= ( const Vec2& _Vec) 
{
	x -= _Vec.x;
	y -= _Vec.y;
    return ( *this ) ;
}

Vec2& Vec2::operator*= ( const float& _f) 
{
	x *= _f;
	y *= _f;

    return ( *this ) ;
}


Vec2& Vec2::operator/= ( const float& _f) 
{
	float fInv = 1.0f / _f;
	x *= fInv;
	y *= fInv; 

    return ( *this ) ;
}

math::Vec2 LerpVec2(const math::Vec2& From, const math::Vec2& To, float t)
{
	if (t < 0.0f) t = 0.0f;
	if (t > 1.0f) t = 1.0f;

	Vec2	Delta = To - From;
	return From + (Delta*t);
}



math::Vec2 GenerateNormal(const math::Vec2& From)
{
	math::Vec2 Result(From.y, -From.x);

	Result.Normalize();

	return Result;
}

Vec2 Vec2::operator + () const
{
	return Vec2(x, y);
}
Vec2 Vec2::operator - () const
{
	return Vec2(-x, -y);
}

Vec2 Vec2::operator + ( const Vec2& v) const
{
	return Vec2(x+v.x, y+v.y);
}
Vec2 Vec2::operator - ( const Vec2& v) const
{
	return Vec2(x-v.x, y-v.y);
}

Vec2 Vec2::operator * ( float c) const
{
	return Vec2(x*c, y*c);
}
Vec2 Vec2::operator / ( float c) const
{
	return Vec2(x/c, y/c);
}

